%% References:
% 1. XinFu Liu，Entry Trajectory Optimization by Second-Order Cone Programming
% 2. 高嘉时，升力式再入飞行器轨迹优化与制导方法研究
% 目前问题：找不到可行解
clc
clear
close all;
[m, S, deg2rad, R0, g0, Vc, rho0, hs, sigmamin, sigmamax, dQmax, CQ, ...
    qmax, nmax, kQ] = getConstant();
[N, epsilon, delta, max_k] = getIteration(R0, deg2rad);

%% 选取初始剖面
rad0 = (1e5 + R0)/R0;
V0 = 7450/Vc;
lon0 = 0;
lat0 = 0;
fpa0 = -0.5*deg2rad;
azi0 = 0;
sigma0 = 0;
e0 = 1/rad0 - V0^2/2;

radf = (25e3 + R0)/R0;
Vf = 625/Vc;
lonf = 12*deg2rad;
latf = 70*deg2rad;
fpaf = -10*deg2rad;
azif = 90*deg2rad;
ef = 1/radf - Vf^2/2;

rad00 = linspace(rad0, radf, N+1); % 待修改：选取航迹倾角由负值翻转到正值的飞行高度作为转折点
lon00 = linspace(lon0, lonf, N+1);
lat00 = linspace(lat0, latf, N+1);
fpa00 = linspace(fpa0, fpa0, N+1);
azi00 = linspace(azi0, azif, N+1);
u100 = linspace(1, 1, N);
u200 = linspace(0, 0, N);
e = linspace(e0, ef, N+1);
delta_e = (ef - e0)/N;

% res
res = cell(max_k,8); % 状态量5+控制量2+obj
res{1,1} = rad00;
res{1,2} = lon00;
res{1,3} = lat00;
res{1,4} = fpa00;
res{1,5} = azi00;
res{1,6} = u100;
res{1,7} = u200;
%% SOCP 迭代求解
for k = 1:max_k % 第k次迭代
    if(k > 1)
        % 判断是否收敛
        diff = [
            max(abs(res{k,1} - res{k-1,1}));
            max(abs(res{k,2} - res{k-1,2}));
            max(abs(res{k,3} - res{k-1,3}));
            max(abs(res{k,4} - res{k-1,4}));
            max(abs(res{k,5} - res{k-1,5}));
            ];
        if(all(diff <= epsilon))
            break % 收敛就退出迭代
        end
    end
    % 替换剖面
    radk = res{k,1};
    lonk = res{k,2};
    latk = res{k,3};
    fpak = res{k,4};
    azik = res{k,5};
    u1k = res{k,6};
    u2k = res{k,7};
    xk = [radk;lonk;latk;fpak;azik];
    rhok = rho0*exp(-R0*(radk - 1)/hs);
    rhokr = -R0*rhok/hs;
    Vk = sqrt(2*(1./radk - e));
    [CL, CD, ~, ~] = getCLCD(Vk, Vc, N, deg2rad);
    Drk =  0.5*R0.*rhokr.*Vk.^2*S.*CD/m;
    % 决策变量
    rad = sdpvar(1,N+1);
    lon = sdpvar(1,N+1);
    lat = sdpvar(1,N+1);
    fpa = sdpvar(1,N+1);
    azi = sdpvar(1,N+1);
    u1 = sdpvar(1,N);
    u2 = sdpvar(1,N);
    delta_lon = sdpvar(1, 1);
    delta_lat = sdpvar(1, 1);
    % 端点约束
    F = [];
    F = [F, rad(1) == rad0];
    F = [F, lon(1) == lon0];
    F = [F, lat(1) == lat0];
    F = [F, fpa(1) == fpa0];
    F = [F, azi(1) == azi0];
    F = [F, rad(N+1) == radf];
    F = [F, lon(N+1) <= delta_lon + lonf, lon(N+1) >= -delta_lon + lonf, delta_lon >= 0];
    F = [F, lat(N+1) <= delta_lat + latf, lat(N+1) >= -delta_lat + latf, delta_lat >= 0];
    F = [F, fpa(N+1) >= fpaf];
    F = [F, azi(N+1) == azif];
    for j = 1:N
        rhoj = rhok(j);
        Vj = Vk(j);
        Lkj = 0.5*R0*rhoj*Vj^2*S*CL(j)/m;
        Dkj = 0.5*R0*rhoj*Vj^2*S*CD(j)/m;
        radkj = radk(j);
        lonkj = lonk(j);
        latkj = latk(j);
        fpakj = fpak(j);
        azikj = azik(j);
        xkj = [radkj; lonkj; latkj; fpakj; azikj];
        Drkj = Drk(j);
        f0xkj = [sin(fpakj)/Dkj;
                 cos(fpakj)*sin(azikj)/(radkj*Dkj*cos(latkj));
                 cos(fpakj)*cos(azikj)/(radkj*Dkj);
                 (Vj^2 - 1/radkj)*cos(fpakj)/radkj/(Vj^2*Dkj);
                 cos(fpakj)*sin(azikj)*tan(latkj)/radkj/Dkj;
                ];
        % A矩阵系数
        a11 = - Drkj*sin(fpakj)/(Dkj^2);
        a12 = 0;
        a13 = 0;
        a14 = cos(fpakj)/Dkj;
        a15 = 0;
        a21 = -(Drkj/radkj/(Dkj^2) + 1/radkj^2/Dkj)*cos(fpakj)*sin(azikj)/cos(latkj);
        a22 = 0;
        a23 = cos(fpakj)*sin(azikj)*sin(latkj)/radkj/Dkj/cos(latkj)^2;
        a24 = -sin(fpakj)*sin(azikj)/radkj/Dkj/cos(latkj);
        a25 = cos(fpakj)*cos(azikj)/radkj/Dkj/cos(latkj);
        a31 = -(Drkj/radkj/(Dkj^2) + 1/radkj^2/Dkj)*cos(fpakj)*cos(azikj);
        a32 = 0;
        a33 = 0;
        a34 = -sin(fpakj)*cos(azikj)/radkj/Dkj;
        a35 = -cos(fpakj)*sin(azikj)/radkj/Dkj;
        a41 = Drkj*cos(fpakj)*(1/radkj/Vj^2-1)+cos(fpakj)*(2/radkj/Vj^2-1)/Dkj/radkj^2;
        a42 = 0;
        a43 = 0;
        a44 = sin(fpakj)*(1/radkj/Vj^2-1)/Dkj/radkj;
        a45 = 0;
        a51 = -cos(fpakj)*sin(azikj)*tan(latkj)*(Drkj/radkj/Dkj^2 + 1/radkj^2/Dkj);
        a52 = 0;
        a53 = cos(fpakj)*sin(azikj)/radkj/Dkj/cos(latkj)^2;
        a54 = -sin(fpakj)*sin(azikj)*tan(latkj)/radkj/Dkj;
        a55 = cos(fpakj)*cos(azikj)*tan(latkj)/radkj/Dkj;
        Akj = [a11, a12 , a13, a14, a15;
               a21, a22 , a23, a24, a25;
               a31, a32 , a33, a34, a35;
               a41, a42 , a43, a44, a45;
               a51, a52 , a53, a54, a55];
        bxkj = f0xkj - Akj*xkj;
        Bkj = [ 0, 0;
                0, 0;
                0, 0;
                Lkj/(Dkj*Vj^2), 0;
                0, Lkj/(Dkj*Vj^2)];
        % 动力学约束 欧拉法离散化
        xj1 = [rad(j+1);lon(j+1);lat(j+1);fpa(j+1);azi(j+1)];
        xj = [rad(j);lon(j);lat(j);fpa(j);azi(j)];
        uj = [u1(j);u2(j)];
        F = [F, xj1 == (Akj*delta_e + eye(5))*xj + delta_e*Bkj*[u1(j);u2(j)] + delta_e*bxkj];
        % 控制量约束
        F = [F, norm(uj, 2) <= 1; u1(j) <= cos(sigmamin); u1(j) >= cos(sigmamax)];
        F = [F, rad(j) >= 1 - 2*hs*log(dQmax/(Vj^3.2)/sqrt(rho0)/kQ/(g0*R0)^1.6)/R0];
        F = [F, rad(j) >= 1 - hs*log(2*qmax/g0/R0/(Vj^2)/rho0)/R0];
        F = [F, rad(j) >= 1 - hs*log(nmax/sqrt(Lkj^2 + Dkj^2))/R0];
        F = [F, xj <= delta + xkj, xj >= -delta + xkj];
    end

    % 构造目标函数
    a = 2*m/R0/S./CD./Vk.^3;
    obj = delta_lon + delta_lat + sum(radk.*(-a.*rhokr)./rhok.^2 + a.*(1./rhok + radk.*rhokr./rhok.^2));

    % SOCP 子问题求解
    ops = sdpsettings('solver','mosek','verbose',1);
    solvesdp(F, obj, ops);

    % 保存每次迭代结果
    res{k+1,1} = value(rad);
    res{k+1,2} = value(lon);
    res{k+1,3} = value(lat);
    res{k+1,4} = value(fpa);
    res{k+1,5} = value(azi);
    res{k+1,6} = value(u1);
    res{k+1,7} = value(u2);
    res{k+1,8} = double(obj);
end